/*
 * EffectsCalculator.cpp
 *
 *  Created on: 27.01.21
 *      Author: Jon Lidgard, Yannick Richter, Vincent Manoukian
 */

#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include "EffectsCalculator.h"
#include "Axis.h"
#include "eeprom.h"
#include "eeprom_addresses.h"

#define X_AXIS_ENABLE 1
#define Y_AXIS_ENABLE 2
#define Z_AXIS_ENABLE 4
#define DIRECTION_ENABLE(AXES) (1 << AXES)

#define EFFECT_STATE_INACTIVE 0


void EC_calculateEffects(EffectsCalculator* ec,Axis* axes,uint8_t axisCount);
void EC_calcStatsEffectType(EffectsCalculator* ec,uint8_t type, int16_t force);
int32_t EC_calcConditionEffectForce(EffectsCalculator* ec,FFB_Effect *effect, float  metric, uint8_t gain,
										 uint8_t idx, float scale, float angle_ratio);
void EC_checkFilterCoeff(EffectsCalculator* ec,biquad_constant_t *filter, uint32_t freq,uint8_t q);
void EC_updateFilterSettingsForEffects(EffectsCalculator* ec,uint8_t type_effect);
int32_t EC_calcNonConditionEffectForce(EffectsCalculator* ec,FFB_Effect *effect);
int32_t EC_calcComponentForce(EffectsCalculator* ec,FFB_Effect *effect, int32_t forceVector, Axis* axes, uint8_t axis, uint8_t axisCount);
int32_t EC_calcComponentForce(EffectsCalculator* ec,FFB_Effect *effect, int32_t forceVector, Axis* axes, uint8_t axis, uint8_t axisCount);
int32_t EC_getEnvelopeMagnitude(EffectsCalculator* ec,FFB_Effect *effect);
float EC_speedRampupPct(EffectsCalculator* ec);


void EC_Init(EffectsCalculator* ec)
{
	ec->global_gain = 0xff;
}


bool EC_isActive(EffectsCalculator* ec)
{
	return ec->effects_active;
}
void EC_setActive(EffectsCalculator* ec,bool active)
{
	ec->effects_active = active;
}

/**
 * Sets the mask where the direction enable bit is in the effect
 */
void EC_setDirectionEnableMask(EffectsCalculator* ec,uint8_t mask)
{
	ec->directionEnableMask = mask;
}

/*
If the metric is less than CP Offset - Dead Band, then the resulting force is given by the following formula:
		force = Negative Coefficient * (q - (CP Offset – Dead Band))
Similarly, if the metric is greater than CP Offset + Dead Band, then the resulting force is given by the
following formula:
		force = Positive Coefficient * (q - (CP Offset + Dead Band))
A spring condition uses axis position as the metric.
A damper condition uses axis velocity as the metric.
An inertia condition uses axis acceleration as the metric.

If the number of Condition report blocks is equal to the number of axes for the effect, then the first report
block applies to the first axis, the second applies to the second axis, and so on. For example, a two-axis
spring condition with CP Offset set to zero in both Condition report blocks would have the same effect as
the joystick self-centering spring. When a condition is defined for each axis in this way, the effect must
not be rotated.

If there is a single Condition report block for an effect with more than one axis, then the direction along
which the parameters of the Condition report block are in effect is determined by the direction parameters
passed in the Direction field of the Effect report block. For example, a friction condition rotated 45
degrees (in polar coordinates) would resist joystick motion in the northeast-southwest direction but would
have no effect on joystick motion in the northwest-southeast direction.
 */

/*
 * Calculates the resulting torque for FFB effects
 * Takes current position input scaled from -0x7fff to 0x7fff
 * Outputs a torque value from -0x7fff to 0x7fff (not yet clipped)
 */
void EC_calculateEffects(EffectsCalculator* ec,Axis* axes,uint8_t axisCount)
{
	
	for (int i=0;i<axisCount;i++)
	{
		AxisCalculateAxisEffects(&axes[i],EC_isActive(ec));
	}

	if(!EC_isActive(ec))
	{
		return;
	}

	int32_t forceX = 0;
	int32_t forceY = 0;
	int32_t forceVector = 0;
	bool validY = axisCount > 1;
#if MAX_AXIS == 3
	int32_t forceZ = 0;
	bool validZ = axisCount > 2;
#endif
	
	for (uint8_t i = 0; i < EC_STATES_SIZE; i++)
	{
		ec->effects_stats[i].current = 0; 			// Reset active effect forces
	}

	for (uint8_t i = 0; i < MAX_EFFECTS; i++)
	{
		FFB_Effect *effect = &ec->pEffects[i];

		// Filter out inactive effects
		if (effect->state == EFFECT_STATE_INACTIVE)
		{
			continue;
		}
		// Effect activated and not infinite (0 or 0xffff)
		if (effect->duration != FFB_EFFECT_DURATION_INFINITE && effect->duration != 0)
		{
			// Start delay not yet reached
			if(HAL_GetTick() < effect->startTime){
				continue;
			}
			// If effect has expired make inactive
			if (HAL_GetTick() > effect->startTime + effect->duration)
			{
				effect->state = EFFECT_STATE_INACTIVE;
//				EC_calcStatsEffectType(ec,effect->type, 0); 	// record a 0 on the ended force
			}
		}
		// TODO 此处的力要取反？
		forceVector = EC_calcNonConditionEffectForce(ec,effect);

		/*
			If the Direction Enable flag is set then the Axes Enable
			flags are ignored, only one Condition Parameter Block is defined
			and the Direction is applied to the Condition Parameter Block as a
			polar direction.
		 */
		if ((effect->enableAxis & ec->directionEnableMask) || (effect->enableAxis & X_AXIS_ENABLE))
		{
			int32_t newEffectForce = EC_calcComponentForce(ec,effect, forceVector, axes, 0,1);
//			EC_calcStatsEffectType(ec,effect->type, newEffectForce);
			forceX += newEffectForce;
			CLIP(forceX,-0x7fff,0x7fff);
		}
		if (validY && ((effect->enableAxis & ec->directionEnableMask) || (effect->enableAxis & Y_AXIS_ENABLE)))
		{
			int32_t newEffectForce = EC_calcComponentForce(ec,effect, forceVector, axes, 0,1);
//			EC_calcStatsEffectType(ec,effect->type, newEffectForce);
			forceY += newEffectForce;
			CLIP(forceX,-0x7fff,0x7fff);
		}

	}
	
	AxisSetEffectTorque(&axes[0],forceX);
	if (validY)
	{
		AxisSetEffectTorque(&axes[1],forceY);
	}
}

/**
 * Calculates forces from a non conditional effect
 * Periodic and constant effects
 */
int32_t EC_calcNonConditionEffectForce(EffectsCalculator* ec,FFB_Effect *effect) {
	int32_t force_vector = 0;
	int32_t magnitude = effect->magnitude;

	//目前测试到pCARS有三个特效，但是有两个特效的这个参数是0,这个参数是0的时候是不可能产生特效的。
	if(magnitude == 0)
	{
		return 0;				
	}
	// If using an envelope modulate the magnitude based on time
	if(effect->useEnvelope)
	{
		magnitude = EC_getEnvelopeMagnitude(ec,effect);
	}
	switch (effect->type)
	{
		case FFB_EFFECT_CONSTANT:
		{ // Constant force is just the force
			force_vector = (int32_t)magnitude;
			break;
		}

		case FFB_EFFECT_RAMP:
		{
			uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
			int32_t duration = effect->duration;
			force_vector = (int32_t)effect->startLevel + ((int32_t)elapsed_time * (effect->endLevel - effect->startLevel)) / duration;
			break;
		}

		case FFB_EFFECT_SQUARE:
		{
			uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
			int32_t force = ((elapsed_time + effect->phase) % ((uint32_t)effect->period + 2)) < (uint32_t)(effect->period + 2) / 2 ? -magnitude : magnitude;
			force_vector = force + effect->offset;
			break;
		}

		case FFB_EFFECT_TRIANGLE:
		{
			int32_t force = 0;
			int32_t offset = effect->offset;
			uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
			uint32_t phase = effect->phase;
			uint32_t period = effect->period;
			float periodF = period;

			int32_t maxMagnitude = offset + magnitude;
			int32_t minMagnitude = offset - magnitude;
			uint32_t phasetime = (phase * period) / 35999;
			uint32_t timeTemp = elapsed_time + phasetime;
			float remainder = timeTemp % period;
			float slope = ((maxMagnitude - minMagnitude) * 2) / periodF;
			if (remainder > (periodF / 2))
				force = slope * (periodF - remainder);
			else
				force = slope * remainder;
			force += minMagnitude;
			force_vector = force;
			break;
		}

		case FFB_EFFECT_SAWTOOTHUP:
		{
			float offset = effect->offset;
			uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
			uint32_t phase = effect->phase;
			uint32_t period = effect->period;
			float periodF = effect->period;

			float maxMagnitude = offset + magnitude;
			float minMagnitude = offset - magnitude;
			int32_t phasetime = (phase * period) / 35999;
			uint32_t timeTemp = elapsed_time + phasetime;
			float remainder = timeTemp % period;
			float slope = (maxMagnitude - minMagnitude) / periodF;
			force_vector = (int32_t)(minMagnitude + slope * (period - remainder));
			break;
		}

		case FFB_EFFECT_SAWTOOTHDOWN:
		{
			float offset = effect->offset;
			uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
			float phase = effect->phase;
			uint32_t period = effect->period;
			float periodF = effect->period;

			float maxMagnitude = offset + magnitude;
			float minMagnitude = offset - magnitude;
			int32_t phasetime = (phase * period) / 35999;
			uint32_t timeTemp = elapsed_time + phasetime;
			float remainder = timeTemp % period;
			float slope = (maxMagnitude - minMagnitude) / periodF;
			force_vector = (int32_t)(minMagnitude + slope * (remainder)); // reverse time
			break;
		}

		case FFB_EFFECT_SINE:
		{
			float t = HAL_GetTick() - effect->startTime;
			float freq = 1.0f / (float)(effect->period>100?effect->period:100);
			float phase = (float)effect->phase / (float)35999; //degrees
			float sine = sin(2.0 * M_PI * (t * freq + phase)) * magnitude;
			force_vector = (int32_t)(effect->offset + sine);
			break;
		}
		default:
			return 0;
			break;
	}

	return (force_vector * effect->gain) / 255.0 * ec->scaler.periodic;
}

/*
 * If the number of Condition report blocks is equal to the number of axes for the effect, then the first report
block applies to the first axis, the second applies to the second axis, and so on. For example, a two-axis
spring condition with CP Offset set to zero in both Condition report blocks would have the same effect as
the joystick self-centering spring. When a condition is defined for each axis in this way, the effect must
not be rotated.

If there is a single Condition report block for an effect with more than one axis, then the direction along
which the parameters of the Condition report block are in effect is determined by the direction parameters
passed in the Direction field of the Effect report block. For example, a friction condition rotated 45
degrees (in polar coordinates) would resist joystick motion in the northeast-southwest direction but would
have no effect on joystick motion in the northwest-southeast direction.
 */

int32_t EC_calcComponentForce(EffectsCalculator* ec,FFB_Effect *effect, int32_t forceVector, Axis* axes, uint8_t axis, uint8_t axisCount)
{
	int32_t result_torque = 0;
	uint16_t direction;
	uint8_t con_idx = 0; // condition block index

	metric_t *metrics = AxisGetMetrics(&axes[axis]);

	if (effect->enableAxis & ec->directionEnableMask)
	{
		direction = effect->directionX;
		con_idx = axis;
	}
	else
	{
		direction = axis == 0 ? effect->directionX : effect->directionY;
		con_idx = axis;
	}

	float angle;
	float angle_ratio;
	if(axisCount > 1)
	{
		angle = ((float)direction * (2*M_PI) / 36000.0);
		angle_ratio = axis == 0 ? sin(angle) : -1 * cos(angle);
	}
	else	//只有一个轴的时候
	{
		angle_ratio = 1.0;
	}
	
	switch (effect->type)
	{
	case FFB_EFFECT_CONSTANT:
	{
		// Optional filtering to reduce spikes,if the filter is enabled we apply it
		if ( BQ_GetFc(&effect->filter[con_idx]) != 0.0 && BQ_GetFc(&effect->filter[con_idx]) < 0.5)
		{
			forceVector = BQ_Process(&effect->filter[con_idx],forceVector);
		}
	}
	case FFB_EFFECT_RAMP:
	case FFB_EFFECT_SQUARE:
	case FFB_EFFECT_TRIANGLE:
	case FFB_EFFECT_SAWTOOTHUP:
	case FFB_EFFECT_SAWTOOTHDOWN:
	case FFB_EFFECT_SINE:
	{
		result_torque = forceVector * angle_ratio;
		break;
	}

	case FFB_EFFECT_SPRING:
	{
		/** 	      |	  (rampup is from 0..5% of max velocity)
		 * 			  |	  __________ (after use max coefficient)
		 * 			  |	 /
		 *			  |	/
		 *			  |-
		 * ------------------------  Velocity
		 * 			 -|
		 *			/ |
		 * 		   /  |
		 * 	-------   |
		 * 			  |
		 */
		float pos = metrics->posDegrees;
		float springForce = pos*INTERNAL_SCALER_SPRING;
		if(effect->conditions[con_idx].deadBand < 2*INTERNAL_SCALER_SPRING)
		{
			effect->conditions[con_idx].deadBand = 2*INTERNAL_SCALER_SPRING;
		}
		result_torque = EC_calcConditionEffectForce(ec,effect, springForce, ec->gain.spring, con_idx, ec->scaler.spring, angle_ratio);
		break;
	}



	case FFB_EFFECT_FRICTION: // TODO sometimes unstable.
	{
		float speed = metrics->speed * INTERNAL_SCALER_FRICTION;
		result_torque = BQ_Process(&effect->filter[con_idx],EC_calcConditionEffectForce(ec,effect, speed, ec->gain.friction, con_idx, ec->scaler.friction, angle_ratio));
		/*
		float speed = metrics->speed * INTERNAL_SCALER_FRICTION;

		int16_t offset = effect->conditions[con_idx].cpOffset;
		int16_t deadBand = effect->conditions[con_idx].deadBand < 200 ? 200:effect->conditions[con_idx].deadBand ;
		int32_t force = 0;

		float speedRampupCeil = EC_speedRampupPct(ec);

		// Effect is only active outside deadband + offset
		if (abs((int32_t)speed - offset) > deadBand){

			// remove offset/deadband from metric to compute force
			speed -= (offset + (deadBand * (speed < offset ? -1 : 1)) );

			// check if speed is in the 0..x% to rampup, if is this range, apply a sinusoidale function to smooth the torque (slow near 0, slow around the X% rampup
			float rampupFactor = 1.0;
			if (fabs (speed) < speedRampupCeil) {								// if speed in the range to rampup we apply a sinus curbe to ramup

				float phaseRad = M_PI * ((fabs (speed) / speedRampupCeil) - 0.5);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode
				rampupFactor = ( 1 + sin(phaseRad ) ) / 2;						// sin value is -1..1 range, we translate it to 0..2 and we scale it by 2

			}

			int8_t sign = speed >= 0 ? 1 : -1;
			uint16_t coeff = speed < 0 ? effect->conditions[con_idx].negativeCoefficient : effect->conditions[con_idx].positiveCoefficient;
			force = coeff * rampupFactor * sign;

			//if there is a saturation, used it to clip result
			if (effect->conditions[con_idx].negativeSaturation !=0 || effect->conditions[con_idx].positiveSaturation !=0) {
				force = force<-effect->conditions[con_idx].negativeSaturation?-effect->conditions[con_idx].negativeSaturation:\
				force>effect->conditions[con_idx].negativeSaturation?effect->conditions[con_idx].positiveSaturation:force;
			}

			result_torque = BQ_Process(&effect->filter[con_idx], (((ec->gain.friction + 1) * force) >> 8) * angle_ratio * ec->scaler.friction);
		}
		*/
		break;
	}
	case FFB_EFFECT_DAMPER:
	{

		float speed = metrics->speed * INTERNAL_SCALER_DAMPER;
		result_torque = BQ_Process(&effect->filter[con_idx],EC_calcConditionEffectForce(ec,effect, speed, ec->gain.damper, con_idx, ec->scaler.damper, angle_ratio));

		break;
	}

	case FFB_EFFECT_INERTIA:
	{
		float accel = metrics->accel * INTERNAL_SCALER_INERTIA;
		result_torque = BQ_Process(&effect->filter[con_idx],EC_calcConditionEffectForce(ec,effect, accel, ec->gain.inertia, con_idx, ec->scaler.inertia, angle_ratio)); // Bump *60 the inertia feedback

		break;
	}

	default:
		// Unsupported effect
		break;
	}
	return (result_torque * ec->global_gain) / 255; // Apply global gain
}

float EC_speedRampupPct(EffectsCalculator* ec) 
{
	return (ec->frictionPctSpeedToRampup / 100.0) * 32767;	// compute the normalizedSpeed of pctToRampup factor
}


/**
 * Calculates a conditional effect
 * Takes care of deadband and offsets and scalers
 * Gain of 255 = 1x. Prescale with scale factor
 */
int32_t EC_calcConditionEffectForce(EffectsCalculator* ec,FFB_Effect *effect, float  metric, uint8_t gain,
										 uint8_t idx, float scale, float angle_ratio)
{
	int16_t offset = effect->conditions[idx].cpOffset;
	int16_t deadBand = effect->conditions[idx].deadBand;
	int32_t force = 0;
	float gainfactor = (float)(gain+1) / 256.0;

	// Effect is only active outside deadband + offset
	if (fabsf(metric - offset) > deadBand){
		float coefficient = 0;
		if(metric > offset)
		{
			coefficient = effect->conditions[idx].positiveCoefficient;
		}
		else
		{
			coefficient = effect->conditions[idx].negativeCoefficient;
		}
		coefficient /= 0x7fff; // rescale the coefficient of effect

		// remove offset/deadband from metric to compute force
		metric = metric - (offset + (deadBand * (metric < offset ? -1 : 1)) );
		
		force = coefficient * gainfactor * scale * (float)(metric);
		CLIP(force,-effect->conditions[idx].negativeSaturation,effect->conditions[idx].positiveSaturation);
	}


	return force * angle_ratio;
}

/**
 * Modulates the magnitude of an effect based on time and attack/fade levels
 * During attack time the strength changes from the initial attack level to the normal magnitude which is sustained
 * until the fade time where the strength changes to the fade level until the stop time of the effect.
 * Infinite effects can't have an envelope and return the normal magnitude.
 */
int32_t EC_getEnvelopeMagnitude(EffectsCalculator* ec,FFB_Effect *effect)
{
	if(effect->duration == FFB_EFFECT_DURATION_INFINITE || effect->duration == 0)
	{
		return effect->magnitude; // Effect is infinite. envelope is invalid
	}
	int32_t scaler = abs(effect->magnitude);
	uint32_t elapsed_time = HAL_GetTick() - effect->startTime;
	if (elapsed_time < effect->attackTime && effect->attackTime != 0)
	{
		scaler = (scaler - effect->attackLevel) * elapsed_time;
		scaler /= (int32_t)effect->attackTime;
		scaler += effect->attackLevel;
	}
	if (elapsed_time > (effect->duration - effect->fadeTime) && effect->fadeTime != 0)
	{
		scaler = (scaler - effect->fadeLevel) * (effect->duration - elapsed_time); // Reversed
		scaler /= (int32_t)effect->fadeTime;
		scaler += effect->fadeLevel;
	}
	scaler = signbit(effect->magnitude) ? -scaler : scaler; // Follow original sign of magnitude because envelope has no sign (important for constant force)
	return scaler;
}

void EC_setFilters(EffectsCalculator* ec,FFB_Effect *effect){

	Biquad* filter;
	filter = effect->filter;	//

	switch (effect->type)
	{
		// ec->filterProfileId是为了设置多轴的，目前只有一个轴
	case FFB_EFFECT_DAMPER:
		BQ_SetBiquad(filter,lowpass, ec->filter[ec->filterProfileId].damper.fc/ (float)ec->calcfrequency, ec->filter[ec->filterProfileId].damper.q * ec->qfloatScaler , (float)0.0);

		break;
	case FFB_EFFECT_FRICTION:
		BQ_SetBiquad(filter,lowpass, ec->filter[ec->filterProfileId].friction.fc / (float)ec->calcfrequency, ec->filter[ec->filterProfileId].friction.q * ec->qfloatScaler, (float)0.0);

		break;
	case FFB_EFFECT_INERTIA:
		BQ_SetBiquad(filter,lowpass, ec->filter[ec->filterProfileId].inertia.fc / (float)ec->calcfrequency, ec->filter[ec->filterProfileId].inertia.q * ec->qfloatScaler, (float)0.0);
		break;
	case FFB_EFFECT_CONSTANT:
		BQ_SetBiquad(filter,lowpass, ec->filter[ec->filterProfileId].constant.fc / (float)ec->calcfrequency, ec->filter[ec->filterProfileId].constant.q * ec->qfloatScaler, (float)0.0);
			break;
	default :
		return;
	}

	for (int i=0; i<MAX_AXIS; i++) {
		effect->filter[i] = *filter;
	}
}


void EC_setGain(EffectsCalculator* ec,uint8_t gain)
{
	ec->global_gain = gain;
}

uint8_t EC_getGain(EffectsCalculator* ec) 
{
	return ec->global_gain;
}

void EC_setEffectsArray(EffectsCalculator* ec,FFB_Effect *pEffects)
{
	ec->pEffects = pEffects;
}



void EC_Filter_Write(EffectsCalculator* ec,uint8_t ecNum,uint16_t filterValue)
{
	uint16_t fv = filterValue;
	uint32_t freq = fv & 0x1FF;
	uint8_t q = (fv >> 9) & 0x7F;
	switch(ecNum)
	{
		case FFB_EFFECT_CONSTANT:
			EC_checkFilterCoeff(ec,&(ec->filter[0].constant), freq, q);
			EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_CONSTANT);
			EE_WriteVariable(ADR_FFB_CF_FILTER, fv);
		break;
		
		case FFB_EFFECT_FRICTION:
			EC_checkFilterCoeff(ec,&(ec->filter[0].friction), freq, q);
			EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_FRICTION);
			EE_WriteVariable(ADR_FFB_FR_FILTER, fv);
		break;
		
		case FFB_EFFECT_DAMPER:
			EC_checkFilterCoeff(ec,&(ec->filter[0].damper), freq, q);
			EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_DAMPER);
			EE_WriteVariable(ADR_FFB_DA_FILTER, fv);
		break;
		
		case FFB_EFFECT_INERTIA:
			EC_checkFilterCoeff(ec,&(ec->filter[0].inertia), freq, q);
			EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_INERTIA);
			EE_WriteVariable(ADR_FFB_IN_FILTER, fv);
		break;
	}
}

void EC_Filter_Read(EffectsCalculator* ec,uint8_t ecNum,uint16_t* pfilterValue)
{
	uint16_t* pfv = pfilterValue;
	switch(ecNum)
	{
		case FFB_EFFECT_CONSTANT:
			*pfv = (ec->filter[0].constant.fc & 0x1FF) | (ec->filter[0].constant.q << 9);
		break;
		
		case FFB_EFFECT_FRICTION:
			*pfv = (ec->filter[0].friction.fc & 0x1FF) | (ec->filter[0].friction.q << 9);
		break;
		
		case FFB_EFFECT_DAMPER:
			*pfv = (ec->filter[0].damper.fc & 0x1FF) | (ec->filter[0].damper.q << 9);
		break;
		
		case FFB_EFFECT_INERTIA:
			*pfv = (ec->filter[0].inertia.fc & 0x1FF) | (ec->filter[0].inertia.q << 9);
		break;
	}
}

void EC_Gain_Scaler_Write(EffectsCalculator* ec,uint8_t ecNum,uint16_t gains)
{
	uint16_t gs = gains;
	switch(ecNum)
	{
		case FFB_EFFECT_SPRING:
			ec->gain.spring = (gs>>8)&0xFF;
			ec->scaler.spring = gs&0xFF;
			EE_WriteVariable(ADR_FFB_SP_GAINS, gs);
		break;
		
		case FFB_EFFECT_FRICTION:
			ec->gain.friction = (gs>>8)&0xFF;
			ec->scaler.friction = gs&0xFF;
			EE_WriteVariable(ADR_FFB_FR_GAINS, gs);
		break;
		
		case FFB_EFFECT_DAMPER:
			ec->gain.damper = (gs>>8)&0xFF;
			ec->scaler.damper = gs&0xFF;
			EE_WriteVariable(ADR_FFB_DA_GAINS, gs);
		break;
		
		case FFB_EFFECT_INERTIA:
			ec->gain.inertia = (gs>>8)&0xFF;
			ec->scaler.inertia = gs&0xFF;
			EE_WriteVariable(ADR_FFB_IN_GAINS, gs);
		break;
		
		case FFB_EFFECT_SINE:
			ec->gain.periodic = (gs>>8)&0xFF;
			ec->scaler.periodic = gs&0xFF;
			EE_WriteVariable(ADR_FFB_PF_GAINS, gs);
		break;
	}
}

void EC_Gain_Scaler_Read(EffectsCalculator* ec,uint8_t ecNum,uint16_t* pgains)
{
	uint16_t* pgs = pgains;
	switch(ecNum)
	{
		case FFB_EFFECT_SPRING:
			*pgs = (ec->gain.spring << 8) | ((uint16_t)ec->scaler.spring);
		break;
		
		case FFB_EFFECT_FRICTION:
			*pgs = (ec->gain.friction << 8) | ((uint16_t)ec->scaler.friction);
		break;
		
		case FFB_EFFECT_DAMPER:
			*pgs = (ec->gain.damper << 8) | ((uint16_t)ec->scaler.damper);
		break;
		
		case FFB_EFFECT_INERTIA:
			*pgs = (ec->gain.inertia << 8) | ((uint16_t)ec->scaler.inertia);
		break;
		
		case FFB_EFFECT_SINE:
			*pgs = (ec->gain.periodic << 8) | ((uint16_t)ec->scaler.periodic);
		break;
	}
}

/*
 * Read parameters from flash and restore settings
 */
void EC_restoreFlash(EffectsCalculator* ec)
{
	
	uint16_t axisId = 0;
	if (!EE_ReadVariable(ADR_FFB_AXIS_ID, &axisId))
	{

	}
	
	uint16_t filterStorage = 0;
	if (!EE_ReadVariable(ADR_FFB_CF_FILTER, &filterStorage))
	{
		uint32_t freq = filterStorage & 0x1FF;
		uint8_t q = (filterStorage >> 9) & 0x7F;
		EC_checkFilterCoeff(ec,&(ec->filter[0].constant), freq, q);
		EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_CONSTANT);
	}

	if (!EE_ReadVariable(ADR_FFB_FR_FILTER, &filterStorage))
	{
		uint32_t freq = filterStorage & 0x1FF;
		uint8_t q = (filterStorage >> 9) & 0x7F;
		EC_checkFilterCoeff(ec,&(ec->filter[CUSTOM_PROFILE_ID].friction), freq, q);
		EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_FRICTION);
	}

	if (!EE_ReadVariable(ADR_FFB_DA_FILTER, &filterStorage))
	{
		uint32_t freq = filterStorage & 0x1FF;
		uint8_t q = (filterStorage >> 9) & 0x7F;
		EC_checkFilterCoeff(ec,&(ec->filter[CUSTOM_PROFILE_ID].damper), freq, q);
		EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_DAMPER);
	}

	if (!EE_ReadVariable(ADR_FFB_IN_FILTER, &filterStorage))
	{
		uint32_t freq = filterStorage & 0x1FF;
		uint8_t q = (filterStorage >> 9) & 0x7F;
		EC_checkFilterCoeff(ec,&(ec->filter[CUSTOM_PROFILE_ID].inertia), freq, q);
		EC_updateFilterSettingsForEffects(ec,FFB_EFFECT_INERTIA);
	}

	uint16_t gainscaler = 0;
	if(!EE_ReadVariable(ADR_FFB_FR_GAINS, &gainscaler))
	{
		ec->gain.friction = (gainscaler >> 8) & 0xff;
		ec->scaler.friction = (gainscaler & 0xff);
	}

	if(!EE_ReadVariable(ADR_FFB_DA_GAINS, &gainscaler))
	{
		ec->gain.damper = (gainscaler >> 8) & 0xff;
		ec->scaler.damper = (gainscaler & 0xff);
	}

	if(!EE_ReadVariable(ADR_FFB_IN_GAINS, &gainscaler))
	{
		ec->gain.inertia = (gainscaler >> 8) & 0xff;
		ec->scaler.inertia = (gainscaler & 0xff);
	}

	
	if(!EE_ReadVariable(ADR_FFB_SP_GAINS, &gainscaler))
	{
		ec->gain.spring = (gainscaler >> 8) & 0xff;
		ec->scaler.spring = (gainscaler & 0xff);
	}
	
	if(!EE_ReadVariable(ADR_FFB_PF_GAINS, &gainscaler))
	{
		ec->gain.periodic = (gainscaler >> 8) & 0xff;
		ec->scaler.periodic = (gainscaler & 0xff);
	}


}

// Saves parameters to flash
void EC_saveFlash(EffectsCalculator* ec)
{
	uint16_t filterStorage;

	// save CF biquad
	filterStorage = (uint16_t)ec->filter[0].constant.fc & 0x1FF;
	filterStorage |= ( (uint16_t)ec->filter[0].constant.q & 0x7F ) << 9 ;
	EE_WriteVariable(ADR_FFB_CF_FILTER, filterStorage);

	if(ec->filterProfileId == CUSTOM_PROFILE_ID){ // Only attempt saving if custom profile active
		// save Friction biquad
		filterStorage = (uint16_t)ec->filter[CUSTOM_PROFILE_ID].friction.fc & 0x1FF;
		filterStorage |= ( (uint16_t)ec->filter[CUSTOM_PROFILE_ID].friction.q & 0x7F ) << 9 ;
		EE_WriteVariable(ADR_FFB_FR_FILTER, filterStorage);

		// save Damper biquad
		filterStorage = (uint16_t)ec->filter[CUSTOM_PROFILE_ID].damper.fc & 0x1FF;
		filterStorage |= ( (uint16_t)ec->filter[CUSTOM_PROFILE_ID].damper.q & 0x7F ) << 9 ;
		EE_WriteVariable(ADR_FFB_DA_FILTER, filterStorage);

		// save Inertia biquad
		filterStorage = (uint16_t)ec->filter[CUSTOM_PROFILE_ID].inertia.fc & 0x1FF;
		filterStorage |= ( (uint16_t)ec->filter[CUSTOM_PROFILE_ID].inertia.q & 0x7F ) << 9 ;
		EE_WriteVariable(ADR_FFB_IN_FILTER, filterStorage);
	}

	// save the effect gain
	uint16_t gainscaler = 0;
	gainscaler = (uint16_t)ec->scaler.friction | (ec->gain.friction << 8);
	EE_WriteVariable(ADR_FFB_FR_GAINS, gainscaler);

	gainscaler = (uint16_t)ec->scaler.damper | (ec->gain.damper << 8);
	EE_WriteVariable(ADR_FFB_DA_GAINS, gainscaler);
	
	gainscaler = (uint16_t)ec->scaler.inertia | (ec->gain.inertia << 8);
	EE_WriteVariable(ADR_FFB_IN_GAINS, gainscaler);
	
	gainscaler = (uint16_t)ec->scaler.spring | (ec->gain.spring << 8);
	EE_WriteVariable(ADR_FFB_SP_GAINS, gainscaler);
	
	gainscaler = (uint16_t)ec->scaler.periodic | (ec->gain.periodic << 8);
	EE_WriteVariable(ADR_FFB_PF_GAINS, gainscaler);

}

void EC_checkFilterCoeff(EffectsCalculator* ec,biquad_constant_t *filter, uint32_t freq,uint8_t q)
{
	if(q == 0) {
		q = 1;
	}

	if(freq == 0){
		freq = ec->calcfrequency / 2;
	}
	
	freq = freq<1?1:freq>ec->calcfrequency/2?ec->calcfrequency/2:freq;
	q = q>127?127:q;
	filter->fc = freq;
	filter->q = q;
}

void EC_updateFilterSettingsForEffects(EffectsCalculator* ec,uint8_t type_effect) {

	// loop on all effect in memory and setup new constant filter
	for (uint8_t i = 0; i < MAX_EFFECTS; i++)
	{
		if (ec->pEffects[i].type == type_effect)
		{
			EC_setFilters(ec,&ec->pEffects[i]);
		}
	}
}


//void EC_logEffectType(EffectsCalculator* ec,uint8_t type,bool remove){
//	if(type > 0 && type < 32){
//
//		if(remove){
//			if(ec->effects_stats[type-1].nb > 0)
//				ec->effects_stats[type-1].nb--;
//
//			if(!ec->effects_stats[type-1].nb){
//				//effects_used &= ~(1<<(type-1)); // Only manual reset
//				//effects_stats[type-1].max = 0;
//				ec->effects_stats[type-1].current = 0;
//			}
//		}else{
//			ec->effects_used |= 1<<(type-1);
//			if( ec->effects_stats[type-1].nb < 65535 ) {
//				ec->effects_stats[type-1].nb ++;
//			}
//		}
//
//	}
//}
//
//void EC_logEffectState(EffectsCalculator* ec,uint8_t type,uint8_t state){
//	if(type > 0 && type < 32){
//		if(!state){
//			// effects_stats[type-1].max = 0;
//			ec->effects_stats[type-1].current = 0;
//		}
//	}
//}
//
//
//void EC_calcStatsEffectType(EffectsCalculator* ec,uint8_t type, int16_t force){
//	if(type > 0 && type < 13) {
//		uint8_t arrayLocation = type - 1;
//		ec->effects_stats[arrayLocation].current += force;
//		ec->effects_stats[arrayLocation].max = ec->effects_stats[arrayLocation].max>(int16_t)abs(force)?ec->effects_stats[arrayLocation].max:(int16_t)abs(force);
//	}
//}
//
//
///**
// * Resets the effects_used flags
// * If reinit is true it will set the flag again if the currently active effect number is not 0
// */
//void EC_resetLoggedActiveEffects(EffectsCalculator* ec,bool reinit){
//	ec->effects_used = 0;
//	if(reinit){
//		for (int i=0;i < 12; i++) {
//			if(ec->effects_stats[i].nb > 0) {
//				ec->effects_used |= 1<<(i);
//			}
//		}
//	}
//}


